#include <opencv2/opencv.hpp>  
#include <iostream>
#include <vector>
#include "../include/common.hpp"

using namespace cv;  
using namespace std;

class imageprocess
{
public: 

    vector<Point>left_jump;                 //赛道左边点集合
    vector<Point>right_jump;                //赛道右边点集合
    vector<uint16_t>trackwidth;             //赛道宽度集合
    int left_lose_count;                    //左边丢线计数器
    int right_lose_count;                   //右边丢线计数器 

    int dutypwm = 1500;
    void Detectionline(Mat& roi_img)
    {	
        r_upflag=false;	
        l_upflag=false;	
        left_lose_count  = 0;                   //左边丢线计数器清零
        right_lose_count = 0;                   //右边丢线计数器清零 
        left_jump.clear();                      //清空数组
        right_jump.clear();                     //清空数组
        trackwidth.clear();
        lastmid = mid;
        for (uint8_t y = ROWSIMAGE-cutimgdown; y > cutimgup; y--)  //开始逐行遍历
        {  
            whitenum = 0;
            // 向左遍历              
            for (uint16_t x = lastmid; x > 1; x--) 
            {  
                if(roi_img.at<uchar>(y, x) == 255) whitenum++;
                if(x <= 2)
                {
                    left_lose_count++;
                    leftpoint = Point(x, y);
                    break;
                }                
                else if (roi_img.at<uchar>(y, x+2)==255 && roi_img.at<uchar>(y, x+1)==255
                 && roi_img.at<uchar>(y, x)==0 && roi_img.at<uchar>(y, x-1)==0) 
                {  
                    leftpoint = Point(x, y);
                    break; 
                }
            }
            // 向右遍历  
            for (uint16_t x = lastmid; x < COLSIMAGE; x++) 
            { 
                if(roi_img.at<uchar>(y, x) == 255) whitenum++;
                if(x >= COLSIMAGE-2) 
                {
                    right_lose_count++;
                    rightpoint = Point(x,y);
                    break;
                }     
                if (roi_img.at<uchar>(y, x-2)==255 && roi_img.at<uchar>(y, x-1)==255
                 && roi_img.at<uchar>(y, x)==0 && roi_img.at<uchar>(y, x+1)==0) 
                {  
                    rightpoint = Point(x,y);
                    break;
                }  
            }
            if(whitenum<20 && y<100) break;//判断到截至行  
            left_jump.push_back(leftpoint);
            right_jump.push_back(rightpoint);
            trackwidth.push_back(rightpoint.x-leftpoint.x);
            lastmid = (leftpoint.x+rightpoint.x)/2; 
        }
        
         if(right_lose_count>145 && left_lose_count<50 && cirstate==0)
         {
                buzzer=true;
                rightcir = true;
                leftcir = false;
                r_up_point = Point(0,0);
                imgnum = 0;
                cirstate = 1;
         }
            cirdeal();
            if(right_lose_count>80&&left_lose_count>80 &&(cirstate==4||cirstate==0))       //十字路口检测
            {
               crossdeal();
            }
    }

    void cirdeal()
    {
        
        switch (cirstate) 
        {
            case 0:  break;
            case 1:
            {
                if(rightcir&&!leftcir)   //右入环岛
                {
                    for(int i=servoline-20;i<servoline+20;i++)
                    {
                    right_jump[i].x = 260;
                    left_jump[i].x = 260;
                    }
                    imgnum++; 
                }
                if(!rightcir&&leftcir)   //左入环岛
                {
                    imgnum++; 
                    for(int i=servoline-20;i<servoline+20;i++)
                    {
                        right_jump[i].x = 10;
                        left_jump[i].x = 10;
                    }
                }
                if(imgnum>45) 
                {
                    imgnum=0;
                    cirstate=2;
                }
            }
            break; 
            case 2:
            {
                if(left_lose_count>30&&right_lose_count>30)
                {
                    imgnum++;
                    if(imgnum>3)
                    {
                    buzzer = true;
                    imgnum=0;
                    cirstate=3;
                    }
                }
            } 
            break;
            case 3:
            {
                if(rightcir&&!leftcir)   //右入环岛
                {
                    imgnum++;
                    for(int i=servoline-20;i<servoline+20;i++)
                    {
                    right_jump[i].x = 260;
                    left_jump[i].x = 260;
                    } 
                }
                if(!rightcir&&leftcir)   //左入环岛
                {
                    imgnum++;
                    for(int i=servoline-20;i<servoline+20;i++)
                    {
                    right_jump[i].x = 70;
                    left_jump[i].x = 70;
                    } 
                }                   
                if(imgnum>55) 
                {
                    imgnum=0;
                    cirstate=4;
                    buzzer = true;
                }
            }
            break;
            case 4:break;        
        }
      

    }
    void crossdeal(void)
    {
        buzzer = true;
        uint8_t l_down_num,l_up_num;
        uint8_t r_down_num,r_up_num;
        for(uint8_t i=5;i<trackwidth.size()/2;i++)
        {
            if((trackwidth[i+5] - trackwidth[i] >20) && left_jump[i].x>10&&left_jump[i+5].x<5)
                { l_down_num = i;
                break;}

            else l_down_num=5;
        }
        for(uint8_t i=left_jump.size()-11;i>20;i--)
        {
            if((trackwidth[i-15] - trackwidth[i] >10) && left_jump[i].x>10&&left_jump[i-15].x<5)
                { l_up_num = i;
                l_upflag = true;
                break;}
        }
        for(uint8_t i=5;i<trackwidth.size()/2;i++)
        {
            if((trackwidth[i+10] - trackwidth[i] >10) && right_jump[i].x<310&&right_jump[i+10].x>315)
                { r_down_num = i;
                break;}
                else r_down_num=5;
        }
        for(uint8_t i=trackwidth.size()-5;i>20;i--)
        {
            if((trackwidth[i-15] - trackwidth[i] >20) && right_jump[i].x<310&&right_jump[i-15].x>315)
                { r_up_num = i;
                  r_upflag = true;
                break;}
        }
        uint8_t leftbunum = l_up_num-l_down_num;
        uint8_t rightbunum = r_up_num-r_down_num;

        l_down_point = left_jump[l_down_num];
        l_up_point = left_jump[l_up_num];
        r_down_point = right_jump[r_down_num];
        r_up_point = right_jump[r_up_num];
        if(l_upflag&&leftbunum&&l_up_point.x>l_down_point.x)
        {        
            vector<Point> l_newPoints = interpolatePoints(l_down_point, l_up_point, leftbunum);
            left_jump.erase(left_jump.begin() + l_down_num, left_jump.begin() + l_up_num);
            left_jump.insert(left_jump.begin() + l_down_num, l_newPoints.begin(), l_newPoints.end());
        }
        if(r_upflag&&rightbunum&&r_up_point.x<r_down_point.x)
        {
            vector<Point> r_newPoints = interpolatePoints(r_down_point, r_up_point, rightbunum);
            right_jump.erase(right_jump.begin() + r_down_num, right_jump.begin() + r_up_num);
            right_jump.insert(right_jump.begin() + r_down_num, r_newPoints.begin(), r_newPoints.end());
        }
    }
    
    void servo()
    {
        float cha=0;
        double pjcha = 0;
        double pjcha_last;
        kp = kp1;
        for(int i=servoline;i<servoline+10;i++)
        {
            cha += (160 - (left_jump[i].x+right_jump[i].x)/2);              //与中线差值
        }
        pjcha = cha /10;  
        int pid = kp*pjcha + kd1*(pjcha-pjcha_last);
        pjcha_last= pjcha;
        int duty1 = (int)(1500 - pid);
        if(duty1>1800)
        {
            duty1 = 1800;
        }
        if(duty1<1200)
        {
            duty1 = 1200;
        }
        dutypwm = duty1;
    }
    void drawimg(string window_name ,Mat& frame)   //显示函数
    {
        for(uint8_t i=0;i<left_jump.size();i++)
        {
            circle(frame, left_jump[i], 2, Scalar(0,255,0), FILLED);  
            circle(frame, right_jump[i], 2, Scalar(0,255,0), FILLED);
            circle(frame, Point((left_jump[i].x+right_jump[i].x)/2,left_jump[i].y), 2, Scalar(255,0,0), FILLED);
        }
        string text = to_string(dutypwm);
        putText(frame, text , Point(10,10), FONT_HERSHEY_PLAIN, 1,Scalar(0, 0, 255), 1);
        string text1 = to_string(kp);
        putText(frame, text1 , Point(10,40), FONT_HERSHEY_PLAIN, 1,Scalar(0, 0, 255), 1);
        circle(frame, l_down_point, 3, Scalar(0,0,255), FILLED);
        circle(frame, l_up_point, 3, Scalar(0,0,255), FILLED);
        circle(frame, r_down_point, 3, Scalar(0,0,255), FILLED);
        circle(frame, r_up_point, 3, Scalar(0,0,255), FILLED);  
        if(r_upflag||l_upflag) 
        {putText(frame, "crossing", Point(80, 30),FONT_HERSHEY_PLAIN, 2, Scalar(255, 0, 0), 1); 
        } 
        imshow(window_name, frame); 
    }

private: 

    bool rightcir=false,leftcir=false;
    bool r_upflag=false,l_upflag=false;
    uint8_t rightpointi;
    uint8_t cirstate = 0;
    int mid    = COLSIMAGE/2 ;              //图像中点横坐标
    int lastmid = mid;                      //巡线的中心
    int endline = cutimgup;                 //巡线截至行

    Point l_down_point,l_up_point,l_mid_point;
    Point r_down_point,r_up_point,r_mid_point;
    Point leftpoint;                         //巡线点：左
    Point rightpoint;                        //巡线点：右
    int whitenum=0;
    float kp;
    int imgnum;

    uint8_t detemid(vector<Point>leftpoint,vector<Point>rightpoint)
    {
        float cha=0;
        float pjcha = 0;
        for(int i=105;i<115;i++)
        {
            cha += (160 - (leftpoint[i].x+rightpoint[i].x)/2);              //与中线差值
        } 
        pjcha = cha /10;
        return abs(pjcha);      
    }

    bool deteline(vector<Point>detepoint)  //TRUE为直线
    {
        int kx1=0,kx2=0;
        kx1 = (detepoint[detepoint.size()-10].y-detepoint[10].y)/(detepoint[detepoint.size()-10].x-detepoint[10].x);
        kx2 = (detepoint[detepoint.size()/2].y-detepoint[10].y)/(detepoint[detepoint.size()/2].x-detepoint[10].x);
        if(abs(kx1-kx2)<3)   return true;
        return false;
    }  

    vector<Point> interpolatePoints(const Point& p1, const Point& p2, int n) 
    {  //补线函数
    vector<Point> points; 
    double dx = static_cast<double>(p2.x - p1.x) / (n + 1);  
    for (int i = 0; i <= n; ++i) 
    {  
        points.emplace_back(static_cast<int>(p1.x + i * dx), static_cast<int>(p1.y - i));  
    }  
    return points;  
    } 
};